#include "icp.h"

ICP::ICP( CGMesh& s, CGMesh& t ): source(s), target(t) {
    assert( source.vert.size() == target.vert.size() );
    output = NULL;

    //Compute barycenter of source
    CGPoint source_barycenter(0.0, 0.0, 0.0);
    for ( int i = 0; i < source.vert.size(); i ++ )
        source_barycenter += source.vert[i].P();
    source_barycenter /= source.vert.size();
    Eigen::Vector3f p_avg( source_barycenter.X(), source_barycenter.Y(), source_barycenter.Z() );

    //Compute barycenter of target
    CGPoint target_barycenter(0.0, 0.0, 0.0);
    for ( int i = 0; i < target.vert.size(); i ++ )
        target_barycenter += target.vert[i].P();
    target_barycenter /= target.vert.size();
    Eigen::Vector3f q_avg( target_barycenter.X(), target_barycenter.Y(), target_barycenter.Z() );

    //Now create covariance matrix (initially zero)
    Eigen::Matrix3f covariance = Eigen::MatrixXf::Zero(3,3);

    for ( int i = 0; i < source.vert.size(); i ++ ) {

        //create vector for source point
        Eigen::Vector3f pi( source.vert[i].P().X() - source_barycenter.X(),
                            source.vert[i].P().Y() - source_barycenter.Y(),
                            source.vert[i].P().Z() - source_barycenter.Z() );


        //create vector for target point
        Eigen::Vector3f qi( target.vert[i].P().X() - target_barycenter.X(),
                            target.vert[i].P().Y() - target_barycenter.Y(),
                            target.vert[i].P().Z() - target_barycenter.Z() );

        //increase covariance
        covariance = covariance + ( pi * (qi.transpose()) );
    }

    //Decompose
    Eigen::JacobiSVD<Eigen::MatrixXf> svd(covariance, Eigen::ComputeFullU | Eigen::ComputeFullV );
    Eigen::Matrix3f U = svd.matrixU();
    Eigen::Matrix3f V = svd.matrixV();

    //get rotation
    rot = U * (V.transpose());

    //get translation
    tra = p_avg - rot* q_avg;

    ///Save results in output mesh
    error = 0.0f;

    for ( int i = 0; i < source.vert.size(); i++ ) {
        //create vector for source point
        Eigen::Vector3f pi( source.vert[i].P().X(),
                            source.vert[i].P().Y(),
                            source.vert[i].P().Z());


        //create vector for target point
        Eigen::Vector3f qi( target.vert[i].P().X(),
                            target.vert[i].P().Y(),
                            target.vert[i].P().Z());

        Eigen::Vector3f current = rot*qi + tra;

        error += ( pi - rot*qi - tra ).squaredNorm();

    }

    //qDebug() << "Global error: " << error;
}

Eigen::Matrix3f ICP::getRot() {
    return rot;
}


Eigen::Vector3f ICP::getTra() {
    return tra;
}

float ICP::getError() {
    return error;
}

/**
 * Create and return an output mesh obtained by shifing the vertices of the source mesh
 **/
CGMesh* ICP::getShiftedTarget() {

    //clear output mesh
    //output.Clear();

    if (output != NULL) free(output);

    output = new CGMesh();

    //copy data from target mesh
    vcg::tri::Append<CGMesh, CGMesh>::Mesh(*output, target);

    //for each point of the target, compute the new position
    for ( int i = 0; i < target.vert.size(); i++ ) {
        //create vector for target point
        Eigen::Vector3f qi( target.vert[i].P().X(),
                            target.vert[i].P().Y(),
                            target.vert[i].P().Z());

        Eigen::Vector3f current = rot*qi + tra;

        output->vert[i].P() = CGPoint( current(0), current(1), current(2) );
    }

    return output;

}

void ICP::moveTarget() {

    //for each point of the target, compute the new position
    for ( int i = 0; i < target.vert.size(); i++ ) {
        //create vector for target point
        Eigen::Vector3f qi( target.vert[i].P().X(),
                            target.vert[i].P().Y(),
                            target.vert[i].P().Z());

        Eigen::Vector3f current = rot*qi + tra;

        target.vert[i].P() = CGPoint( current(0), current(1), current(2) );
    }

}
